import os
import launch
import launch_ros
import launch.event_handlers
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
    # 获取参数文件路径
    pkg_dir = get_package_share_directory('task_server_pkg')
    param_file_path = os.path.join(pkg_dir, 'params', 'task_server_params.yaml')
    
    lifecycle_nodes = ['task_server']
    start_lifecycle_manager_cmd = launch_ros.actions.Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_task_server',
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}])

    
    task_server_cmd = Node(
        package='task_server_pkg',
        executable='task_server',
        name='task_server',
        output='screen',
        parameters=[param_file_path]
    )

    ld = LaunchDescription()
    ld.add_action(start_lifecycle_manager_cmd)
    ld.add_action(task_server_cmd)
    
    return ld
